/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.chromedome.subsystems;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.Subsystem;
import ticks.chromedome.OI;
import ticks.chromedome.RobotMap;
import ticks.chromedome.commands.DriveWithJoystick;

/**
 *
 * @author Jared
 */
public class Drive extends Subsystem {
    private static Drive instance = null;
    private RobotDrive drive;
    private Counter wheel_encoder;
    
     public static Drive getInstance() {
        if(instance == null){
            instance = new Drive();   
        }
        return instance;
    }
     private Drive(){
         drive = new RobotDrive(RobotMap.DRIVE_LEFT_MOTOR, RobotMap.DRIVE_RIGHT_MOTOR);
         drive.setSafetyEnabled(false);
         wheel_encoder = new Counter(RobotMap.DRIVE_OPTICAL);
}
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
     public void driveWithJoystick(){
        drive.arcadeDrive(OI.getInstance().getDriveStick()); 
     }
     public double getWheelEncoder(){
         return wheel_encoder.get();
     }
     public void resetWheelEncoder(){
         wheel_encoder.reset();
     }
     public void setDrive(double move, double rotate){
         drive.arcadeDrive(move, rotate);
     }
     public void driveStop(){
         drive.arcadeDrive(0,0);
     }
     public String debugDrive(){
       
         return ("Drive speed " + Double.toString(1/wheel_encoder.getPeriod()));
     }
    public void initDefaultCommand() {
        setDefaultCommand(new DriveWithJoystick());
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
}
